#include "World.h"

#include "Remote.h"
#include "Track.h"

World* World::_instance = 0;
float THRESHOLD = .9;

World::World() {}


World::~World() {
}

void World::init(){
	BZRC* com = Remote::instance();

	// TODO: This may not be the best way of updating these
	_constants.clear();
	_flags.clear();
	_tanks.clear();
	_obstacles.clear();
	_other_tanks.clear();
	_shots.clear();
	_teams.clear();

	std::vector<Command> init;
	init.push_back(Command(-1, -1, -1, false, false, CONSTANTS | OBSTACLES | TEAMS | MY_TANKS | OTHER_TANKS | FLAGS | SHOTS));
	com->do_commands(init);

	_occgrids.resize(_tanks.size());
	init.clear();
	for(int i = 0; i < _tanks.size(); i ++)
		init.push_back(Command(i, -1, -1, false, true));
	com->do_commands(init);

	for(unsigned int i = 0; i < _constants.size(); i ++){
		if(_constants[i].name.compare("worldsize") == 0){
			size = std::atoi(_constants[i].value.c_str());
		}
		if(_constants[i].name.compare("truepositive") == 0){
			truePositive = std::atof(_constants[i].value.c_str());
		}
		if(_constants[i].name.compare("truenegative") == 0){
			trueNegative = std::atof(_constants[i].value.c_str());
		}
	}	

	grid = new OccupancyProbabilityGrid(getSize(), 0.3f);

	for(int i = 0; i < _tanks.size(); i ++)
		grid->update(&_occgrids[i]);

	g = new Graph(8);
	g->buildEdges();
	g->clearSearchFlags();
	for(int i = 0; i < _tanks.size(); i ++)
		g->updateFromProbabilityGrid(&_occgrids[i]);
	t = new Track();
}

World* World::instance() {
	if (!_instance)
		_instance = new World();

	return _instance;
}

bool World::isCorner(Vector2 pos) {
	/*bool tl = false,
		 tm = false,
		 tr = false,
		 ml = false,
		 mr = false,
		 bl = false,
		 bm = false,
		 br = false,
		 ME = false;

	if (grid->at(pos.x, pos.y) > THRESHOLD) {
		ME = true;
	}
	
	if (pos.x > 0 && pos.y > 0) {
		if (grid->at(pos.x - 1, pos.y - 1) > THRESHOLD) {
			tl = true;
		}
	}
	
	if (pos.x > 0) {
		if (grid->at(pos.x - 1, pos.y) > THRESHOLD) {
			ml = true;
		}
	}

	if (pos.x > 0 && pos.y < getSize() - 1) {
		if (grid->at(pos.x - 1, pos.y + 1) > THRESHOLD) {
			bl = true;
		}
	}

	if (pos.y > 0) {
		if (grid->at(pos.x, pos.y - 1) > THRESHOLD) {
			tm = true;
		}
	}

	if (pos.y < getSize() - 1) {
		if (grid->at(pos.x, pos.y + 1) > THRESHOLD) {
			bm = true;
		}
	}

	if (pos.x < getSize() - 1 && pos.y > 0) {
		if (grid->at(pos.x + 1, pos.y - 1) > THRESHOLD) {
			tr = true;
		}
	}

	if (pos.x < getSize() - 1) {
		if (grid->at(pos.x + 1, pos.y) > THRESHOLD) {
			mr = true;
		}
	}

	if (pos.x < getSize() - 1 && pos.y < getSize() - 1) {
		if (grid->at(pos.x + 1, pos.y + 1) > THRESHOLD) {
			br = true;
		}
	}

	if (!tl && !tm && !tr &&
		!ml &&  ME &&  mr &&
		!bl &&  bm &&  br) {
			return true;
	}

	if (!tl && !tm && !tr &&
		 ml &&  ME && !mr &&
		 bl &&  bm && !br) {
			 return true;
	}

	if (!tl &&  tm &&  tr &&
		!ml &&  ME &&  mr &&
		!bl && !bm && !br) {
			return true;
	}

	if ( tl &&  tm && !tr &&
		 ml &&  ME && !mr &&
		!bl && !bm && !br) {
			return true;
	}

	if (!tl &&  tm &&  tr &&
		 ml &&  ME &&  mr &&
		 bl &&  bm &&  br) {
			 return true;
	}

	if ( tl &&  tm && !tr &&
		 ml &&  ME &&  mr &&
		 bl &&  bm &&  br) {
			 return true;
	}

	if ( tl &&  tm &&  tr &&
		 ml &&  ME &&  mr &&
		!bl &&  bm &&  br) {
			return true;
	}

	if ( tl &&  tm &&  tr &&
		 ml &&  ME &&  mr &&
		 bl &&  bm && !br) {
			 return true;
	}

	return false;*/

bool myTemplate[8][25] = {{1,1,1,0,0,
						  1,1,1,0,0,
						  1,1,1,0,0,
						  0,0,0,0,0,
						  0,0,0,0,0},
						{0,0,1,1,1,
						  0,0,1,1,1,
						  0,0,1,1,1,
						  0,0,0,0,0,
						  0,0,0,0,0},
						{0,0,0,0,0,
						  0,0,0,0,0,
						  1,1,1,0,0,
						  1,1,1,0,0,
						  1,1,1,0,0},
						{0,0,0,0,0,
						  0,0,0,0,0,
						  0,0,1,1,1,
						  0,0,1,1,1,
						  0,0,1,1,1},
						{1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,0,0,
						  1,1,1,0,0},
						{1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1,
						  0,0,1,1,1,
						  0,0,1,1,1},
						{1,1,1,0,0,
						  1,1,1,0,0,
						  1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1},
						{0,0,1,1,1,
						  0,0,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1,
						  1,1,1,1,1}};

	int temp[25] = {0,0,0,0,0,
					 0,0,0,0,0,
					 0,0,0,0,0,
					 0,0,0,0,0,
					 0,0,0,0,0};
	
	float THRESHOLD2 = .15;

	int minx = max(pos.x-2, 0)-pos.x,
		maxx = min(pos.x+2, World::instance()->getSize() - 1) - pos.x;

	int miny = max(pos.y-2, 0)-pos.y,
		maxy = min(pos.y+2, World::instance()->getSize() - 1) - pos.y;

	for(int i = minx; i <= maxx; i ++){
		for(int j = miny; j <= maxy; j ++){
			if(i == 0 && j ==0){
				if (grid->at(pos.x, pos.y) > THRESHOLD) {
					temp[12] = 1;
				}
			}
			else{
				if (grid->at(pos.x + i, pos.y + j) > THRESHOLD2) {
					temp[(2+i) * 5 + (2+j)] = 1;
				}
			}
		}
	}

	int count = 0;
	for(int i = 0; i < 8; i ++){
		count = 0;
		for(int j = 0; j < 25; j ++){
			if(temp[j] != myTemplate[i][j])
				count ++;
		}
		if(count < 2)
			return true;
	}
	return false;
}

vector<Vector2> World::findObstacleCorners(Vector2 start_pos) {
	return vector<Vector2>();
}

Command World::update() {
	BZRC* com = Remote::instance();

	/*
	vector <Vector2> corners;

	for (unsigned int i = 0; i < getSize(); i++) {
		for (unsigned int j = 0; j < getSize(); j++) {
			Vector2 pos(i, j);
			if (isCorner(pos)) {
				corners.push_back(pos);
			}
		}
	}
	*/

	return Command(-1, -1, -1, false, false, MY_TANKS | OTHER_TANKS | FLAGS | SHOTS);
}

void World::printCorners(){
	vector <Vector2> corners;

	grid->cleanup();

	for (unsigned int i = 0; i < getSize(); i++) {
		for (unsigned int j = 0; j < getSize(); j++) {
			Vector2 pos(i, j);
			if (isCorner(pos)) {
				corners.push_back(pos);
			}
		}
	}

	std::vector< std::vector< Vector2 > > obstacles = floodFill( corners );

	for(int i = 0; i < obstacles.size(); i ++){
		std::cout << "Obstacle " << i << ":" << endl;
		for(int j = 0; j < obstacles[i].size(); j ++){
			std::cout << "\t(" << obstacles[i][j].x << "," << obstacles[i][j].y << ")" << endl;
		}
	}
}

std::vector<Vector2> World::getCorners(){
	vector <Vector2> corners;

	for (unsigned int i = 0; i < getSize(); i++) {
		for (unsigned int j = 0; j < getSize(); j++) {
			Vector2 pos(i, j);
			if (isCorner(pos)) {
				corners.push_back(Vector2(pos.x-size/2, pos.y-size/2));
			}
		}
	}
	return corners;
}

std::vector<Vector2> World::getCleanup(){
	return t->getCleanupPoints();
}

std::vector< std::vector< Vector2 > > World::floodFill( std::vector< Vector2 > corners ){
	std::vector< std::vector< Vector2 > > obstacles;
	int* filled = new int[size*size];
	Vector2* stack = new Vector2[size*size];
	int pos = 0;
	Vector2 tmp;

	for(int i = 0; i < size*size; i ++)
		filled[i] = -1;

	int cur;
	int ngbrs[8][2];

	for(int i = 0; i < corners.size(); i ++){
		cur = corners[i].x*size+corners[i].y;
		if(filled[cur] >= 0) {
			obstacles[filled[cur]].push_back(corners[i]);
			continue;
		}
		else{
			std::cout << "filled " << filled[cur] << endl;
		}

		obstacles.push_back(std::vector< Vector2 >());
		obstacles[obstacles.size()-1].push_back(corners[i]);

		stack[0] = corners[i];
		pos ++;

		int count = 0;
		while(pos > 0){
			tmp = stack[pos-1];
			pos --;
			cur = tmp.x*size+tmp.y;
			if(filled[cur] >= 0)
				continue;

			count ++;
			filled[cur] = obstacles.size() - 1;

			ngbrs[0][0] = ngbrs[1][0] = ngbrs[2][0] = ngbrs[3][0] = ngbrs[4][0] = ngbrs[5][0] = ngbrs[6][0] = ngbrs[7][0] = -1;
			ngbrs[0][1] = ngbrs[1][1] = ngbrs[2][1] = ngbrs[3][1] = ngbrs[4][1] = ngbrs[5][1] = ngbrs[6][1] = ngbrs[7][1] = -1;

			if(tmp.x > 0){
				ngbrs[0][0] = tmp.x-1;
				ngbrs[0][1] = tmp.y;
				if(tmp.y > 0){
					ngbrs[4][0] = tmp.x-1;
					ngbrs[4][1] = tmp.y-1;
				}
				if(tmp.y < size-1){
					ngbrs[5][0] = tmp.x-1;
					ngbrs[5][1] = tmp.y+1;
				}
			}
			if(tmp.x < size-1){
				ngbrs[1][0] = tmp.x+1;
				ngbrs[1][1] = tmp.y;
				if(tmp.y > 0){
					ngbrs[6][0] = tmp.x+1;
					ngbrs[6][1] = tmp.y-1;
				}
				if(tmp.y < size-1){
					ngbrs[7][0] = tmp.x+1;
					ngbrs[7][1] = tmp.y+1;
				}
			}
			if(tmp.y > 0){
				ngbrs[2][0] = tmp.x;
				ngbrs[2][1] = tmp.y-1;
			}
			if(tmp.y < size-1){
				ngbrs[3][0] = tmp.x;
				ngbrs[3][1] = tmp.y+1;
			}

			for(int ii = 0; ii < 4; ii ++){
				if(ngbrs[ii][0] >= 0){
					if(filled[ngbrs[ii][0]*size+ngbrs[ii][1]] > 0 ||
						grid->at(ngbrs[ii][0],ngbrs[ii][1]) < .9) continue;

					stack[pos] = Vector2(ngbrs[ii][0], ngbrs[ii][1]);
					pos++;
				}
			}
		}
		std::cout << count << endl;
	}

	return obstacles;

	return obstacles;
}

OccupancyProbabilityGrid* World::occupancy_probability_grid() {
	return grid;
}

vector <constant_t> & World::constants() {
	return _constants;
}

vector <flag_t> & World::flags() {
	return _flags;
}

vector <tank_t> & World::tanks() {
	return _tanks;
}

vector <obstacle_t> & World::obstacles() {
	return _obstacles;
}

vector <otank_t> & World::other_tanks() {
	return _other_tanks;
}

vector <shot_t> & World::shots() {
	return _shots;
}

vector <team_t> & World::teams() {
	return _teams;
}
vector <occgrid_t> & World::occgrids(){
	return _occgrids;
}

int World::getSize(){
	return size;
}

float World::getTruePositive(){
	return truePositive;
}

float World::getTrueNegative(){
	return trueNegative;
}

Track* World::getTrack(){
	return t;
}

Graph* World::getGraph(){
	return g;
}

vector <constant_t> * World::constantsP() {
	return &_constants;
}

vector <flag_t> * World::flagsP() {
	return &_flags;
}

vector <tank_t> * World::tanksP() {
	return &_tanks;
}

vector <obstacle_t> * World::obstaclesP() {
	return &_obstacles;
}

vector <otank_t> * World::other_tanksP() {
	return &_other_tanks;
}

vector <shot_t> * World::shotsP() {
	return &_shots;
}

vector <team_t> * World::teamsP() {
	return &_teams;
}

vector <occgrid_t> * World::occgridsP(){
	return &_occgrids;
}